import Controller.*;
import lejos.nxt.*;
import java.io.*;

public class Calibrate {


	public static void main(String[] args) {

		OrangeController oc = new OrangeController();
		oc = new OrangeController(oc.getCirc(),oc.getAxis(),400,500);
		
		LCD.drawString("Wheel circ.",0,0, true );
		while(true) {
			oc.forward(300);
			
			double distance = 0.0;
			double increment = 0.1;
			LCD.drawString("Increment: " + increment + "mm    ",0,2 );
			LCD.drawString("Devation: " + distance + "mm        " ,0,3);

			Buttonloop: {
				while(true) {
					int button = Button.waitForPress();
					switch(button) {
						case 1:
							break Buttonloop;
						case 2:
							distance -= increment;
							break;
						case 4:
							distance += increment;
							break;
						case 8:
							if(increment < 1) increment = 1.0;
							else if(increment < 10) increment = 10.0;
							else increment = 0.1;				
					}
					LCD.drawString("Increment: " + Double.toString(increment) + "mm   ",0,2 );
					LCD.drawString("Deviation: " + Double.toString(distance) + "mm         " ,0,3);
				}
			}
			
			double newWheelCirc = (300.0+(distance/10.0))/(300.0/(oc.getCirc()));
			LCD.drawString("New: " + newWheelCirc + "         ",0,2);
			oc = new OrangeController(newWheelCirc,oc.getAxis(),400,500);
			int b = Button.waitForPress();
			if(b==8) break;
		}
		
		LCD.clearDisplay();
		LCD.drawString("Track width",0,0, true );
		while(true) {
			oc.turnCCW(3600);
			
			double angle = 0;
			double increment = 0.1;
			LCD.drawString("Increment: " + increment + "    ",0,2 );
			LCD.drawString("Devation: " + angle + "        " ,0,3);

			Buttonloop: {
				while(true) {
					int button = Button.waitForPress();
					switch(button) {
						case 1:
							break Buttonloop;
						case 2:
							angle -= increment;
							break;
						case 4:
							angle += increment;
							break;
						case 8:
							if(increment < 1) increment = 1.0;
							else if(increment < 10) increment = 10.0;
							else increment = 0.1;				
					}
					LCD.drawString("Increment: " + Double.toString(increment) + "   ",0,2 );
					LCD.drawString("Deviation: " + Double.toString(angle) + "         " ,0,3);
				}
			}
			
			double newTrackWidth = (oc.getAxis()*3600.0) / (3600.0+angle);
			LCD.drawString("New: " + newTrackWidth + "         ",0,2);
			oc = new OrangeController(oc.getCirc(),newTrackWidth,400,500);
			int b = Button.waitForPress();
			if(b==8) break;
		}
		
		LCD.clearDisplay();
		LCD.drawString("Save calibration?",0,0, true );
		int b = Button.waitForPress();
		try {
			if(b==1) {
				File f = new File("calib.dat");
				if(f.exists()) f.delete();
				if(!f.createNewFile()) throw new IOException();
				FileOutputStream fos = new FileOutputStream(f);
				long bits = Double.doubleToLongBits(oc.getCirc());
				byte[] bytes = new byte[8];
				for(int i=0;i<8;i++) {
					//bytes[i] = (byte)((bits>>>((7-i)*8))&0xff);
					bytes[7-i] = (byte)(bits&0xff);
					bits = bits>>>8;
				}
				for(int i=0;i<8;i++) {
					fos.write(bytes[i]);
				}
				bits = Double.doubleToLongBits(oc.getAxis());
				for(int i=0;i<8;i++) {
					//bytes[i] = (byte)((bits>>>((7-i)*8))&0xff);
					bytes[7-i] = (byte)(bits&0xff);
					bits = bits>>>8;
				}
				for(int i=0;i<8;i++) {
					fos.write(bytes[i]);
				}
				fos.close();
				LCD.drawString("Saved.       ",0,0, true );
			}
		} catch(Exception e) {
			LCD.drawString("Save error.     ",0,0, true );
		}
		Button.waitForPress();
	}
}
